#include "bsp.h"
#include "mrfi.h"
#include "bsp_leds.h"
#include "bsp_buttons.h"
#include "nwk_types.h"
#include "nwk_api.h"
#include "nwk_frame.h"
#include "nwk.h"
#include "vlo_rand.h"
#include "cod_errores.h"
#include "CalcPos.h"
#include "tabla.h"


void Init();
void TXString( char* cadena, int largo );

/** conexiones con cada nodo
*/
static          linkID_t sLinkIDX = 0;
static          linkID_t sLinkIDA = 0;
static          linkID_t sLinkIDB = 0;


static          uint8_t  sRxTid = 0;
static          char comandoSerie = 0;

static uint8_t sRxCallback(linkID_t port);


// main del nodo central
// 
int main( void )
{
  // Stop watchdog timer to prevent time out reset
  WDTCTL = WDTPW + WDTHOLD;
  
  Init();
  
  
  
  // inicializo el kit
  BSP_Init();
  
  
  TXString("Tracking v0.1\n",15);
  
  
  // levanto la radio -----------------------------------------------
  addr_t lAddr;
  
  // direccion del nodo central!!!!
  lAddr.addr[0]=0xAC; // nodo central
  lAddr.addr[1]=0;
  lAddr.addr[2]=0;
  lAddr.addr[3]=10;
  // ------------------------------
  // seteo la direccion del modulo
  // OJO que hay que reprogramar!!!!
  SMPL_Ioctl(IOCTL_OBJ_ADDR, IOCTL_ACT_SET, &lAddr);
  
  
  // esta llamada devuelve un fail, porque no es la idea que levante nada
  // asi que ni la chequeo...
  SMPL_Init(sRxCallback);
  
  
  // termine de levantar la radio
  TXString("Inicializacion finalizada\r\n",27);
  
  // contador para saber cuantas conexiones tengo y parar cuando llego a
  // 3 conexiones, mas adelante va a ser en distinto orden.
  int conexiones = 0;
  
  while (conexiones<3)
  {
    switch(conexiones){
    case 0: 
      // el nodo central busca al nodo A
      if (SMPL_SUCCESS == SMPL_Link(&sLinkIDA))   
      {
        conexiones++;
      }
      break;
    case 1:
      // el nodo central busca al nodo B
      if (SMPL_SUCCESS == SMPL_Link(&sLinkIDB))   
      {
        conexiones++;
      }
      break;
    case 2:
      // el nodo central busca al nodo X
      if (SMPL_SUCCESS == SMPL_Link(&sLinkIDX))   
      {
        conexiones++;
      }
      break; 
    default:
      break;
    }
    BSP_TOGGLE_LED1(); // hago parpadear el led para ver que esta haciendo
    // (algo mas corto que mandar un warning)
  }
  
  // hay conexion
  signalError(0);
  
  
  /*
  uint8_t     msg[MAX_APP_PAYLOAD], len;
  
  msg[0] = 't';
  msg[1] = 'x';
  msg[2] = 'N';
  msg[3] = 'C';
  
  len = 4;
  
  // EMPIEZA EL PROTOCOLO PROPIAMENTE DICHO!
  if(SMPL_SUCCESS == SMPL_Send(sLinkIDX,msg,len)){
    signalError(0);    
  }
  else{
    signalError(4);
  }
  
  len = 0;
  */
  
  // no vuelve nunca mas
  systemStart();
  
  return 0;
}

/////////////////////////////////////////////////////////////////////////////
char espero = 1;

/** largo del mensaje que hay esperando en la cola
*/
char largoMsgX = 0;
char largoMsgA = 0;
char largoMsgB = 0;

/** cola de mensajes de los nodos
*/
char[MAX_APP_PAYLOAD] msgA;
char[MAX_APP_PAYLOAD] msgB;
char[MAX_APP_PAYLOAD] msgX;

//-------------------------------------------------------------------
// Inicio el sistema
/**
*	Loop principal del programa
*	atiende las funciones basandose en la lectura de banderas
*	para evitar problemas con datos compartidos.
*/
void systemStart(){
  
  while(1)
  {
    // espero a que inicien el sistema con un comando,
    // o con el boton.  
    clearError();
    while(1){
      if(BSP_BUTTON1() || comandoSerie){
        signalError(1);
        break;
      }
    }
    
    // me llamaron e intento ubicar a los nodos para pedir info
    // empiezo por el X
    
    char[] msg = "WKUP"; // mensaje de despertarse
    // round robin con interrupciones
    // espero que me conteste
    while(!largoMsgX){
      SMPL_SUCCESS == SMPL_Send(sLinkIDX,msg,len);
    }
    while(!largoMsgA){
      SMPL_SUCCESS == SMPL_Send(sLinkIDA,msg,len);
    }
    while(!largoMsgB){
      SMPL_SUCCESS == SMPL_Send(sLinkIDB,msg,len);
    }
    
    
    // Recolecte la info, ahora tengo que calcular y mostrar
    localizar(RSSIAMETRO(rssi_intX), RSSIAMETRO(rssi_intA), 
              RSSIAMETRO(rssi_intB));
    
    
  }  
}

//-------------------------------------------------------------------
//
void localizar(float distX,float distA, float distB){

  TXString("X:",2);  
  TXString(float2String(),4);
  
  TXString("Y:",2);  
  TXString(float2String(),4);
  
  TXString("\n\r",2); 
}

/** Convierte un float a un string para enviar por puerto serie
*/
char* float2String(float numero){
  
    char* result = "xx.x"";
    int nro = 10*numero;

    result[3] = '0'+nro%10;
    result[1] = '0'+(nro/10)%10;
    result[0] = '0'+(nro/100)%10;
  
  return result;
}

// ------------------------------------------------------------------
// transmito por el puerto serie
//
/**
*	Transmite por el puerto serie
*	se pasa el string como array de char, y el largo
*	de caracteres validos en el array.
*	Si largo es mas grande que el del array, se arma
*/
void TXString( char* cadena, int largo )
{
  int puntero;
  for( puntero = 0; puntero < largo; puntero++){
    UCA0TXBUF = cadena[puntero];
    while (!(IFG2&UCA0TXIFG));              // espero a que termine de enviar
  }
}


/**
	Variables para guardar el ultimo valor del rssi
*/
static int rssi_intX;
static int rssi_intA;
static int rssi_intB;

// -------------------------------------------------
/**
*	Funcion de Callback que se llama en la ISR de la
*	radio. (Requisito de SimpliciTI)
*/
static uint8_t sRxCallback(linkID_t port){
  uint8_t msg[MAX_APP_PAYLOAD], len;
  len = 0;
    ioctlRadioSiginfo_t sigInfo;
    char rssiString[3];
    volatile signed int rssi_int;
    signed char rssi;
  // manejo el mensaje dependiendo de quien lo envia
  if (port == sLinkIDX)
  {
    // si recibo un paquete de largo mayor que 0
    if ((SMPL_SUCCESS == SMPL_Receive(sLinkIDX, msg, &len)) && len)
    {
      sigInfo.lid = sLinkIDX;
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      rssi = (signed char)sigInfo.sigInfo[0];
      rssi_intX = (signed int) rssi;
      rssi_intX += 128;
      largoMsgX = len;
      // guardo el mensaje
      copiarArray(msg,msgX,len);

      
//      rssiString[2] = '0'+(rssi_int%10);
//      rssiString[1] = '0'+((rssi_int/10)%10);
//      rssiString[0] = '0'+((rssi_int/100)%10);
    }
  }
  else if(port == sLinkIDA){
    if ((SMPL_SUCCESS == SMPL_Receive(sLinkIDA, msg, &len)) && len)
    {
      sigInfo.lid = sLinkIDA;
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      rssi = (signed char)sigInfo.sigInfo[0];
      rssi_intA = (signed int) rssi;
      rssi_intA += 128;
      largoMsgA = len;
    }
  }
  else if(port == sLinkIDB){
    if ((SMPL_SUCCESS == SMPL_Receive(sLinkIDB, msg, &len)) && len)
    {
      sigInfo.lid = sLinkIDB;
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      rssi = (signed char)sigInfo.sigInfo[0];
      rssi_intB = (signed int) rssi;
      rssi_intB += 128;
      largoMsgB = len;
    }
  }

  // libero el frame porque lo use aca...
  // guardo el ultimo mensaje, la unica opcion de que me envien
  // mas mensajes es que sean de WKUP y eso pasa solo en los nodos
  // moviles.
  // Como el codigo que ejecuta es corto lo dejo en el contexto
  // de la ISR
  return 1;
}

/** 	Copia un array en otro, hace un "deep copy"
*	el destino debe ser mas grande que el origen
*	sino se arma.
*/
void copiarArray(char* origen,char* destino, int largo ){

	for(int i=0;i<largo;i++){
		destino[i] = origen[i];
	}
}
//---------------------------------------------------------------------------
/** INICIALIZACION DEL SISTEMA
*
*/
void Init()
{
  // SimpliciTI igual los cambia...
  P1DIR = 0xFB;
  P1OUT = 0x00;
  P2DIR = 0x27;
  P2OUT = 0x00;
  P3DIR = 0xC0;
  P3OUT = 0x00;
  P4DIR = 0xFF;
  P4OUT = 0x00;
  
  
  
  BCSCTL1 = CALBC1_8MHZ;                    // Set DCO
  DCOCTL = CALDCO_8MHZ;
  
  BCSCTL3 |= LFXT1S_2;                      // LFXT1 = VLO
  TACCTL0 = CCIE;                           // TACCR0 IE
  TACCR0 = 12000;                           // aprox un seg
  TACTL = TASSEL_1 + MC_1;                  // ACLK, upmode
  
  P3SEL |= 0x30;                            // P3.4,5 = USCI_A0 TXD/RXD
  UCA0CTL1 = UCSSEL_2;                      // SMCLK
  UCA0BR0 = 0x41;                           // 9600 a 8Mhz
  UCA0BR1 = 0x3;
  UCA0MCTL = UCBRS_2;                       
  UCA0CTL1 &= ~UCSWRST;                     // **Initialize USCI state achine**
  IE2 |= UCA0RXIE;                          // Enable USCI_A0 RX interrupt
  __enable_interrupt(); 
}

//---------------------------------------------------------------------/** Timer A0 ISR
*/
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A (void)
{
  // la idea era dormirse despues de x tiempo
}


///--------------------------------------------------------------------/** ISR puerto serie
*/
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
  
  // me preparo para recibir comandos...
  
  char rx = UCA0RXBUF;
  if ( rx == 'b' || rx == 'B' )  // buscar nodo
  {
    comandoSerie = rx;
  }
  else if ( rx == 'S' || rx == 's' ) // S iempre encendido
  {
  }
  else if ( rx == 'A' || rx == 'a' ) // A pagar, o A dormir
  {
  }
  
  
}

